#ifndef _FILTER_H_
#define _FILTER_H_
#include "common_headfile.h"

#define AcceRatio 67.2f
#define GyroRatio 30.0f
#define ACC_FILTER_NUM 5  // 加速度计滤波深度
#define GYRO_FILTER_NUM 5 // 陀螺仪滤波深度

#define PITCHCONSTANT 4 // 重力补偿系数       减小要适当增大D（K2）
#define PITCHDT 0.01   // 积分系数 0.0028   决定了跟踪速度，

#define ROLLCONSTANT 4 // 重力补偿系数       减小要适当增大D（K2）
#define ROLLDT 0.01  // 积分系数 0.0028   决定了跟踪速度，

#define YAWCONSTANT 4 // 重力补偿系数       减小要适当增大D（K2）
#define YAWDT 0.01   // 积分系数 0.0028   决定了跟踪速度，

class LowPassFilter
{
public:
    float Tf;
    float y_prev;
};
float lpf_operator(LowPassFilter LPF, float x, float dt);
void imu_filter();
void X_Roll_Angle_ComplementaryFilter(float G_angle, float Gyro);
void Y_Pitch_Angle_ComplementaryFilter(float G_angle, float Gyro);
void Z_Yaw_Angle_ComplementaryFilter(float G_angle, float Gyro);
#endif